#include "tracker.h"
#include "win_exception.h"
#include <process.h>
#include <iostream>


unsigned int WINAPI tracker_thread(void* arg)
{
  try {
    tracker* t = (tracker*) arg;

    while(true)
    {
      X_WIN_THROW_IF(WaitForSingleObject(t->timer, INFINITE)!=WAIT_OBJECT_0, "tracker_thread: WaitForSingleObject");
      t->chr.start();
      t->track(t->consume_frame());
      t->chr.stop();
    }
    return 0;
  }
  catch(win_exception& e)
  {
    std::cout<<e.what()<<"\nerror number: "<<e.error_number()<<std::endl;
    return e.error_number();
  }
  catch(std::exception& e)
  {
    std::cout<<e.what()<<std::endl;
    return -1;
  }
}

unsigned int WINAPI tracker_cam_thread(void* arg)
{
  try {
    tracker* t = (tracker*) arg;

    while(true)
    {
      //X_WIN_THROW_IF(WaitForSingleObject(t->timer2, INFINITE)!=WAIT_OBJECT_0, "tracker_thread: WaitForSingleObject");
      t->cam_chr.start();
      t->produce_frame();
      t->cam_chr.stop();
    }
    return 0;
  }
  catch(win_exception& e)
  {
    std::cout<<e.what()<<"\nerror number: "<<e.error_number()<<std::endl;
    return e.error_number();
  }
  catch(std::exception& e)
  {
    std::cout<<e.what()<<std::endl;
    return -1;
  }
}


tracker::tracker(size_t period, bool transpose_and_flip, cv::Point2f bias, cv::Point2f full_scale)
  : period(period), cam(CV_CAP_ANY), transpose_and_flip(transpose_and_flip), bias(bias), full_scale(full_scale), chr(period), cam_chr(period)
{
  InitializeCriticalSection(&crit);
  InitializeCriticalSection(&crit_frame);

  timer = CreateWaitableTimer(NULL, FALSE, NULL);
  X_WIN_THROW_IF_NULL(timer, "tracker::tracker: CreateWaitableTimer");
  LARGE_INTEGER Preset;
  Preset.QuadPart = -1;
  X_WIN_THROW_IF_NULL(SetWaitableTimer(timer, &Preset, period, NULL, NULL, FALSE), "tracker::tracker: SetWaitableTimer");

  //timer2 = CreateWaitableTimer(NULL, FALSE, NULL);
  //X_WIN_THROW_IF_NULL(timer2, "tracker::tracker: CreateWaitableTimer");
  //X_WIN_THROW_IF_NULL(SetWaitableTimer(timer2, &Preset, period, NULL, NULL, FALSE), "tracker::tracker: SetWaitableTimer");

  sem_produced = CreateSemaphore(0,0,1,0);
  X_WIN_THROW_IF_NULL(sem_produced, "tracker: CreateSemaphore");
  sem_consumed = CreateSemaphore(0,1,1,0);
  X_WIN_THROW_IF_NULL(sem_consumed, "tracker: CreateSemaphore");
  produce_frame();

  thread = (HANDLE) _beginthreadex(NULL, 0, tracker_thread, this, 0, &threadId);
  X_WIN_THROW_IF_NULL(thread, "tracker: CreateThread");
  X_WIN_THROW_IF_NULL(SetThreadPriority(thread,THREAD_PRIORITY_TIME_CRITICAL), "tracker: SetThreadPriority");

  cam_thread = (HANDLE) _beginthreadex(NULL, 0, tracker_cam_thread, this, 0, &threadId);
  X_WIN_THROW_IF_NULL(cam_thread, "tracker: CreateThread");
  X_WIN_THROW_IF_NULL(SetThreadPriority(cam_thread,THREAD_PRIORITY_TIME_CRITICAL), "tracker: SetThreadPriority");
}

tracker::~tracker()
{
  X_WIN_THROW_IF_NULL(TerminateThread(thread,0), "~tracker: TerminateThread(thread)");
  
  DWORD ret=STILL_ACTIVE;
  while(true)
  {
    X_WIN_THROW_IF_NULL(GetExitCodeThread(thread,&ret), "~tracker: WaitForSingleObject");
    if(ret!=STILL_ACTIVE)
      break;
    Sleep(period);
  }

  X_WIN_THROW_IF_NULL(TerminateThread(cam_thread,0), "~tracker: TerminateThread(cam_thread)");
  
  ret=STILL_ACTIVE;
  while(true)
  {
    X_WIN_THROW_IF_NULL(GetExitCodeThread(cam_thread,&ret), "~tracker: WaitForSingleObject");
    if(ret!=STILL_ACTIVE)
      break;
    Sleep(period);
  }

  X_WIN_THROW_IF_NULL(CloseHandle(thread), "~tracker: CloseHandle(thread)");
  X_WIN_THROW_IF_NULL(CloseHandle(cam_thread), "~tracker: CloseHandle(endThread)");
  X_WIN_THROW_IF_NULL(CloseHandle(timer), "tracker::~tracker: CloseHandle(timer)");
  //X_WIN_THROW_IF_NULL(CloseHandle(timer2), "tracker::~tracker: CloseHandle(timer)");
  X_WIN_THROW_IF_NULL(CloseHandle(sem_produced), "tracker::~tracker: CloseHandle(sem_produced)");
  X_WIN_THROW_IF_NULL(CloseHandle(sem_consumed), "tracker::~tracker: CloseHandle(sem_consumed)");
  DeleteCriticalSection(&crit);
  DeleteCriticalSection(&crit_frame);
}


void tracker::produce_frame()
{
  cv::Mat true_frame;
//  cam_chr.start();
  cam>>true_frame;
//  cam_chr.stop();

  cv::Mat frame;
  if(transpose_and_flip)
  {
    transpose(true_frame,frame);
    flip(frame,frame,-1);
    frame = frame(cv::Rect(0,0,frame.size().width/3,frame.size().height));
  }
  else
    frame = true_frame;

//  X_WIN_THROW_IF(WaitForSingleObject(sem_consumed,INFINITE)!=WAIT_OBJECT_0, "produce_frame: WaitForSingleObject");

  EnterCriticalSection(&crit_frame);
  last_frame = frame;
  //frames.push(frame);
  LeaveCriticalSection(&crit_frame);

//  X_WIN_THROW_IF_NULL(ReleaseSemaphore(sem_produced,1,0), "produce_frame: ReleaseSemaphore");
}

cv::Mat tracker::consume_frame()
{
//  X_WIN_THROW_IF(WaitForSingleObject(sem_produced,INFINITE)!=WAIT_OBJECT_0, "produce_frame: WaitForSingleObject");
  cv::Mat frame;
  EnterCriticalSection(&crit_frame);
  //frame = frames.front();
  //frames.pop();
  last_frame.copyTo(frame);
  LeaveCriticalSection(&crit_frame);
  //last_frame = frame;

//  X_WIN_THROW_IF_NULL(ReleaseSemaphore(sem_consumed,1,0), "produce_frame: ReleaseSemaphore");
  return frame;
}